#!/usr/bin/python

"""
pyode-tutorial-3.py
This is spinning top demo.

Written By:
    James Thomas
    Email: jim@houseoftechnology.org
    Web: http://mission-cognition.houseoftechnology.org/

Copyright 2009

This file is part of the PandaODElib distribution.

This library is free software; you can redistribute it and/or
modify it under the terms of EITHER:
  (1) The GNU Lesser General Public License as published by the Free
      Software Foundation; either version 2.1 of the License, or (at
      your option) any later version. The text of the GNU Lesser
      General Public License is included with this library in the
      file LICENSE.
  (2) The BSD-style license that is included with this library in
      the file LICENSE-BSD.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
LICENSE and LICENSE-BSD for more details. 

Note: The fact that the cylinder passes through the plane is a defect
in ode itself, not the ode3d library.
"""

import random
from math import pi

from pandac.PandaModules import Quat, OdePlaneGeom, Mat4
import ode3d
import ode3d.lib
import pvis

class Top(ode3d.GDMBody):
    def __init__(self, World, Diameter, Height):
        # initialize the parent class
        ode3d.GDMBody.__init__(self, World)
    
        shaft = ode3d.GDMElement().DefineCylinder(2000, Diameter / 100.0, Height)
        shaft.SetCollisionBitfields(2,1) # Category = 1, collide with 0
        self.AddElement('shaft', shaft)
        print 'shaft', shaft._mass.getMagnitude()
        
        disk = ode3d.GDMElement().DefineCylinder(2000, Diameter, Height / 5.0)
        disk.SetCollisionBitfields(1,1) # Category = 0, collide with 0
        disk.setOffsetPosition((0,0, 0.3 * Height)) # Must be called after AddElement
        self.AddElement('disk', disk)
        print 'disk', disk._mass.getMagnitude()

        self.AddElement('cg', ode3d.DisplayElement(pvis.sphere(radius=0.05)))

class MyApp(ode3d.lib.OdeApp):
    def __init__(self):
        fps = 30 # 30 frames per second
        dt = 1/60.0 # the target rate for our sim time step
        gravity = (0,0,-9.81)
        
        # Now we can initialize the pvis.base class
        ode3d.lib.OdeApp.__init__(self, dt, fps, Gravity=gravity)

        # Setup a random error generator
        rand = random.Random()
        error = lambda: rand.gauss(0.0, 0.01)  

        # Add arrows for the world axes and a sphere for the origin
        self.az = pvis.arrow(length=0.5, color=(0.2,0.2,0.5))
        self.origin = pvis.sphere(radius=self.az.shaftwidth/2)
        #self.origin = loader.loadModel('../../resources/sphere.egg')
        #self.origin.setScale((.1, .1, .1))
        #self.origin.reparentTo(render)
        
        # Create a plane geom for the floor that the top will spin on
        self.plane = OdePlaneGeom(self.world.space, 0, 0, 1, 0)

        # Create a display object for the floor
        self.floor = pvis.box(size=(10,10,.0001), color=(.2,.2,.2))
        
        # Create our top
        top = Top(self.world, 1, 1)
        self.AddObj('top', top)

        # Position the top above the plane so it will drop onto it when the sim starts
        top.setPosition((0,0,5))
        
        # Apply a random initial orientation error to our top
        q = Quat(top.getQuaternion())
        qr = Quat(1, error(), error(), error())
        qr.normalize()
        q *= qr
        top.setQuaternion(q)

        # Put an initial spin on our top
        top.setAngularVel((0, 0, 10))

        # Create an initial camera position
        pvis.base.disableMouse()
        pvis.base.camera.setPos(4, 4, 1)
        pvis.base.camera.lookAt(0, 0, 0) #@UndefinedVariable -- lookAt(), more Panda3d magic?

        # Turn mouse control back on
        mat=Mat4(pvis.base.camera.getMat()) #@UndefinedVariable -- getMat(), more Panda3d magic?
        mat.invertInPlace()
        pvis.base.mouseInterfaceNode.setMat(mat)
        pvis.base.enableMouse()

if __name__ == '__main__':
    app = MyApp()
    #app.Run(Static=True)
    app.Run(0)
